//
// Created by chen on 25-4-24.
//

#include "led.h"

#include <cstdio>
#include "../motor/Motor.h"
#include "./tim.h"
#include "adc.h"
#include "../MT6701CT_STD/MT6701CT_STD.h"
#include "../pid/PidCotroler.h"

Led &Led::instance() {
    static Led instance; // 只创建一次
    return instance;
}

Led::Led() {
    MX_GPIO_Init();
    MX_TIM1_Init();
}

void Led::MX_GPIO_Init(void) {
    GPIO_InitTypeDef GPIO_InitStruct = {0};

    /* GPIO Ports Clock Enable */
    __HAL_RCC_GPIOB_CLK_ENABLE();

    /*Configure GPIO pin Output Level */
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET);

    /*Configure GPIO pin : PB15 */
    GPIO_InitStruct.Pin = GPIO_PIN_15;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
}

// 点亮 LED（低电平点亮）
void Led::ledon(void) {
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET);
}

// 熄灭 LED（高电平熄灭）
void Led::ledoff(void) {
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET);
}

/**
  * @brief This function handles TIM1 update interrupt.
  */
// 全局作用域（文件顶部）
extern PidController speedPid;
extern "C" void TIM1_UP_IRQHandler(void) {
    HAL_TIM_IRQHandler(&htim1);
    static bool flag = 0;
    static int count = 0;
    count++;
    if (count == 1000) {
        flag = !flag;
        count = 0;
        if (flag) {
            Led::instance().ledon();
        } else {
            Led::instance().ledoff();
        }
    }

    // ====== 关键改动部分 ======
    static bool inited = false;
    MT6701Sensor *instance = MT6701Sensor::getInstance();

    // 注意：提前 static 定义
    if (!inited) {
        inited = true;
    } else {
        float ang=instance->getRawAngle();
        float  v=instance->getVelocity();
        float fang=instance->getFullAngle();
        uint8_t step = instance->getCommutationStep();

        float control = speedPid.compute(3, v);
        BLDCMotor::getInstance().setSpeed(control);
        BLDCMotor::getInstance().updateStep(step);
        BLDCMotor::getInstance().executeCommutation(BLDCMotor::getInstance().getCommutationStep(step));
        printf("Step: %d  Angle: %.2f  Speed: %.2f\n  fang: %.2f\n" , step, ang, v,fang);
    }
}
